/**
 * @file /include/kobuki_driver/packets/cliff.hpp
 *
 * @brief BaseController sensor packet payloads.
 *
 * License: BSD
 *   https://raw.github.com/yujinrobot/kobuki_core/hydro-devel/kobuki_driver/LICENSE
 */
/*****************************************************************************
** Preprocessor
*****************************************************************************/

#ifndef BASE_CONTROLLER_DATA_HPP__
#define BASE_CONTROLLER_DATA_HPP__

/*****************************************************************************
** Include
*****************************************************************************/
#include "../command.hpp"
#include "../packet_handler/payload_headers.hpp"
#include "../packet_handler/payload_base.hpp"
/*****************************************************************************
** Namespace
*****************************************************************************/

/*****************************************************************************
** Interface
*****************************************************************************/

class BaseController : public payloadBase
{
public:
  BaseController() : payloadBase(false, 4) {};

  struct Data {
    short speed;
    short radius;
  } data;

  bool serialise(deque<unsigned char> & byteStream)
  {
    buildBytes((unsigned char)Command::BaseControl, byteStream);
    buildBytes(speed, byteStream);
    buildBytes(radius, byteStream);
    return true;
  }

  bool deserialise(deque<unsigned char> & byteStream)
  {
    if (byteStream.size() < length+2)
    {
      //std::cout << "kobuki_node: kobuki_cliff: deserialise failed. not enough byte stream." << std::endl;
      return false;
    }

    unsigned char header_id, length_packed;
    buildVariable(header_id, byteStream);
    buildVariable(length_packed, byteStream);
    if( header_id != Command::BaseControl ) return false;
    if( length_packed != length ) return false;

    buildVariable(speed, byteStream);
    buildVariable(radius, byteStream);
    
    //showMe();
    return true;
    //return constrain();
  }

};

#endif /* BASE_CONTROLLER_DATA_HPP__ */
